<?php

// Levenberg-Marquardt in PHP 

// http://www.idiom.com/~zilla/Computer/Javanumeric/LM.java

class LevenbergMarquardt {

  /**
  * Calculate the current sum-squared-error
  *
  * Chi-squared is the distribution of squared Gaussian errors,
  * thus the name.
  *
  * @param double[][] $x
  * @param double[] $a
  * @param double[] $y, 
  * @param double[] $s, 
  * @param object $f
  */
  function chiSquared($x, $a, $y, $s, $f) {

    $npts = count($y);

    $sum = 0.0;

    for($i = 0; $i < $npts; $i++ ) {
      $d = $y[$i] - $f->val($x[$i], $a);
      $d = $d / $s[$i];
      $sum = $sum + ($d*$d);
    }

    return $sum;

  }


  /**
  * Minimize E = sum {(y[k] - f(x[k],a)) / s[k]}^2
  * The individual errors are optionally scaled by s[k].
  * Note that LMfunc implements the value and gradient of f(x,a),
  * NOT the value and gradient of E with respect to a!
  * 
  * @param x array of domain points, each may be multidimensional
  * @param y corresponding array of values
  * @param a the parameters/state of the model
  * @param vary false to indicate the corresponding a[k] is to be held fixed
  * @param s2 sigma^2 for point i
  * @param lambda blend between steepest descent (lambda high) and
  *	jump to bottom of quadratic (lambda zero).
  * 	Start with 0.001.
  * @param termepsilon termination accuracy (0.01)
  * @param maxiter	stop and return after this many iterations if not done
  * @param verbose	set to zero (no prints), 1, 2
  *
  * @return the new lambda for future iterations.
  *  Can use this and maxiter to interleave the LM descent with some other
  *  task, setting maxiter to something small.
  */
  function solve($x, $a, $y, $s, $vary, $f, $lambda, $termepsilon, $maxiter, $verbose) {

    $npts = count($y);
    $nparm = count($a);

    if ($verbose > 0) {
      print("solve x[".count($x)."][".count($x[0])."]");
      print(" a[".count($a)."]");
      println(" y[".count(length)."]");
    }

    $e0 = $this->chiSquared($x, $a, $y, $s, $f);

    //double lambda = 0.001;
    $done = false;

    // g = gradient, H = hessian, d = step to minimum
    // H d = -g, solve for d
    $H = array();
    $g = array();
    
    //double[] d = new double[nparm];

    $oos2 = array();
    
    for($i = 0; $i < $npts; $i++)  $oos2[$i] = 1./($s[$i]*$s[$i]);

    $iter = 0;
    $term = 0;	// termination count test

    do {
      ++$iter;

      // hessian approximation
      for( $r = 0; $r < $nparm; $r++ ) {
	      for( $c = 0; $c < $nparm; $c++ ) {
	        for( $i = 0; $i < $npts; $i++ ) {
	          if ($i == 0) $H[$r][$c] = 0.;
	          $xi = $x[$i];
	          $H[$r][$c] += ($oos2[$i] * $f->grad($xi, $a, $r) * $f->grad($xi, $a, $c));
	        }  //npts
	      } //c
      } //r

      // boost diagonal towards gradient descent
      for( $r = 0; $r < $nparm; $r++ )
	      $H[$r][$r] *= (1. + $lambda);

      // gradient
      for( $r = 0; $r < $nparm; $r++ ) {
	      for( $i = 0; $i < $npts; $i++ ) {
	        if ($i == 0) $g[$r] = 0.;
	        $xi = $x[$i];
	        $g[$r] += ($oos2[$i] * ($y[$i]-$f->val($xi,$a)) * $f->grad($xi, $a, $r));
	      }
      } //npts

      // scale (for consistency with NR, not necessary)
      if ($false) {
	      for( $r = 0; $r < $nparm; $r++ ) {
	        $g[$r] = -0.5 * $g[$r];
	        for( $c = 0; $c < $nparm; $c++ ) {
	          $H[$r][$c] *= 0.5;
	        }
	      }
      }

      // solve H d = -g, evaluate error at new location
      //double[] d = DoubleMatrix.solve(H, g);
      double[] d = (new Matrix(H)).lu().solve(new Matrix(g, nparm)).getRowPackedCopy();
      //double[] na = DoubleVector.add(a, d);
      double[] na = (new Matrix(a, nparm)).plus(new Matrix(d, nparm)).getRowPackedCopy();
      double e1 = chiSquared(x, na, y, s, f);

      if (verbose > 0) {
	      System.out.println("\n\niteration "+iter+" lambda = "+lambda);
	      System.out.print("a = ");
        (new Matrix(a, nparm)).print(10, 2);
	      if (verbose > 1) {
          System.out.print("H = "); 
          (new Matrix(H)).print(10, 2);
          System.out.print("g = "); 
          (new Matrix(g, nparm)).print(10, 2);
          System.out.print("d = "); 
          (new Matrix(d, nparm)).print(10, 2);
	      }
	      System.out.print("e0 = " + e0 + ": ");
	      System.out.print("moved from ");
        (new Matrix(a, nparm)).print(10, 2);
	      System.out.print("e1 = " + e1 + ": ");
	      if (e1 < e0) {
	        System.out.print("to ");
          (new Matrix(na, nparm)).print(10, 2);
	      } else {
	        System.out.println("move rejected");
	      }
      }

      // termination test (slightly different than NR)
      if (Math.abs(e1-e0) > termepsilon) {
	      term = 0;
      } else {
	      term++;
	      if (term == 4) {
	        System.out.println("terminating after " + iter + " iterations");
	        done = true;
	      }
      }
      if (iter >= maxiter) done = true;

      // in the C++ version, found that changing this to e1 >= e0
      // was not a good idea.  See comment there.
      //
      if (e1 > e0 || Double.isNaN(e1)) { // new location worse than before
	      lambda *= 10.;
      } else {		// new location better, accept new parameters
	      lambda *= 0.1;
	      e0 = e1;
	      // simply assigning a = na will not get results copied back to caller
	      for( int i = 0; i < nparm; i++ ) {
	        if (vary[i]) a[i] = na[i];
	      }
      }
    } while(!$done);

    return $lambda;

  } //solve

}

?>
